#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/path.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <nav_msgs/msg/path.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/filter.h>
#include "tf2/LinearMath/Vector3.h"
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/buffer.h>
#include "tf2_ros/transform_listener.h"
#include "tf2_sensor_msgs/tf2_sensor_msgs.h"
#include <tf2_ros/create_timer_ros.h>
#include <tf2_ros/message_filter.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <cmath>

pcl::PointCloud<pcl::PointXYZ>::Ptr mapCloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr laserCloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr laserCloudCrop(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr laserCloudDwz(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudI(new pcl::PointCloud<pcl::PointXYZI>());
//debug 
pcl::PointCloud<pcl::PointXYZ>::Ptr debugCloud(new pcl::PointCloud<pcl::PointXYZ>());

pcl::VoxelGrid<pcl::PointXYZ> laserDwzFilter;
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
class ApfPlanner : public rclcpp::Node
{
  public:
    /* 搜索的步长（路径上两点间的距离，单位：米 */
    double searchStep = 0.2;
    /* 引力常数与斥力常数 */
    double attraction = 20;
    double repulsion = 4;
    /* 认定到达终点的阈值，单位：米 */
    double goalTolerance = 0.2;
    /* 点云体素下采样的分辨率（把直径为多少的体素内的点转为一个点），单位：米。
      （避障的权重，越小对这一层的避障效果越大） */
    double voxelGridResolution = 0.3;
    /* 地图体素下采样的分辨率（把直径为多少的体素内的点转为一个点），单位：米。
      （避障的权重，越小对这一层的避障效果越大） */
    double mapGridResolution = 0.7;
    /* 机器人的半径，单位：米 */
    double robotRange = 0.05;
    /* 障碍物的最低高度，单位：米 */
    double obstacleHeight = 0.6;
    /* 是否开启debug输出，会损失一些性能 */
    bool isDebug = true;
    /* K临近搜索的点的数量 */
    int nearPointsNum = 20;
     /* 以第几个点作为前进方向 */
    int forewardPoint = 3;
    /* 速度增益 */
    double speed=5;
    /* 最低速度 */
    double minSpeed=0.1;
    /* 规划的范围 */
    double planRange = 2.0;
    /* 障碍物斥力阈值（如果比赛就把这个参数调高）（表演就调低这个参数） */
    double inflationPower = 10.0;
    /* 如果在建图 */
    bool isSlam = true;
    // double inflationPower = 15.0;

    bool updateMap = false;

    bool newLaserCloud=false;

    bool hasGoal=false;

    geometry_msgs::msg::PoseStamped tempPose, goalPose;

    nav_msgs::msg::Path globalPath;

    ApfPlanner() : Node("apf_planner")
    {
      publisher_cloud = this->create_publisher<sensor_msgs::msg::PointCloud2>("point_cloud_topic", 10);
      publisher_path = this->create_publisher<nav_msgs::msg::Path>("plan", 10);
      publisher_vel = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);
      timer_ = this->create_wall_timer(std::chrono::milliseconds(10), std::bind(&ApfPlanner::timer_callback, this));
      map_sub = this->create_subscription<nav_msgs::msg::OccupancyGrid>(
              "/inflated_map", 
              10,
              std::bind(&ApfPlanner::topic_callback_map, 
              this, 
              std::placeholders::_1));
      goal_sub = this->create_subscription<nav_msgs::msg::Path>(
              "/path", 
              10,
              std::bind(&ApfPlanner::topic_callback_goal, 
              this, 
              std::placeholders::_1));
      cloud_sub = this->create_subscription<sensor_msgs::msg::PointCloud2>(
              "mid360",
              10,
              std::bind(&ApfPlanner::topic_callback_cloud, 
              this, 
              std::placeholders::_1));

      tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
      tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
      laserDwzFilter.setLeafSize(voxelGridResolution, voxelGridResolution, voxelGridResolution);  
    }

    void topic_callback_goal(const nav_msgs::msg::Path & poses)
    {
      globalPath = poses;
      hasGoal=true;
    }

    void topic_callback_map(const nav_msgs::msg::OccupancyGrid & map)
    {    
      if(!updateMap || isSlam)
      {
        mapCloud->clear();
        /* map data */
        double origin_x, origin_y, resolution;
        int width, height;
        origin_x = map.info.origin.position.x;
        origin_y = map.info.origin.position.y;
        //cout<<origin_x<<origin_y<<endl;
        width = map.info.width;
        height = map.info.height;
        resolution = map.info.resolution;
        //cout<<width<<" "<<height<<" "<<resolution<<endl;
    
        pcl::PointXYZ tempPoint;
    
        for(int i = 0; i < width; i++)
        {
          for(int j = 0; j < height; j++)
          {
            if(map.data[j * width + i] >= 90)
            {
              tempPoint.x = i * resolution + origin_x;
              tempPoint.y = j * resolution + origin_y;
              mapCloud->push_back(tempPoint);
            }
          }
        }
    
        pcl::PointCloud<pcl::PointXYZ>::Ptr temp_ptr(new pcl::PointCloud<pcl::PointXYZ>);
        temp_ptr = mapCloud;
        pcl::VoxelGrid<pcl::PointXYZ> sor;
        sor.setInputCloud(temp_ptr);
        sor.setLeafSize(mapGridResolution, mapGridResolution, mapGridResolution);
        sor.filter(*mapCloud);
    
        updateMap = true;
      }

    }

    void topic_callback_cloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr laserCloud2)
    {
      //等待地图接收
      if(!updateMap)
        return;

      laserCloud->clear();
      
      pcl::fromROSMsg(*laserCloud2, *laserCloud);

      pcl::PointXYZ point;
      laserCloudCrop->clear();
      int laserCloudSize = laserCloud->points.size();
      for (int i = 0; i < laserCloudSize; i++) {
        point = laserCloud->points[i];

        float pointX = point.x;
        float pointY = point.y;
        float pointZ = point.z;

        float dis = sqrt(pointX * pointX + pointY  * pointY);
        //有bug，无法解决：dis < 2.0
        if ( dis > robotRange && pointZ<obstacleHeight) {
          point.x = pointX;
          point.y = pointY;
          point.z = pointZ;
          laserCloudCrop->push_back(point);
        }
      }

      // 坐标转换
      sensor_msgs::msg::PointCloud2 map_cloud_msg;
      pcl::toROSMsg(*laserCloudCrop, map_cloud_msg);
      try {
          geometry_msgs::msg::TransformStamped transform = tf_buffer_->lookupTransform("map", "base_link", tf2::TimePointZero);
          tf2::doTransform(map_cloud_msg, map_cloud_msg, transform);
      } catch (tf2::TransformException& ex) {
          RCLCPP_ERROR(this->get_logger(), "Error transforming point cloud: %s", ex.what());
          return;
      }
      pcl::fromROSMsg(map_cloud_msg, *laserCloudCrop);

      // 最终输出
      laserCloudDwz->clear();
      laserDwzFilter.setInputCloud(laserCloudCrop);
      laserDwzFilter.filter(*laserCloudDwz);
      *laserCloudDwz=*laserCloudDwz+*mapCloud;
      
      // calculateSlopeAndDirection();
      newLaserCloud = true;

      //debug
      if(isDebug)
      {
        sensor_msgs::msg::PointCloud2 ros2_cloud_map;
        pcl::toROSMsg(*laserCloudDwz, ros2_cloud_map);
        ros2_cloud_map.header.frame_id = "map";
        publisher_cloud->publish(ros2_cloud_map);
      }
    }

    // 计算当前点与目标点的距离
    double distanceToGoal(const geometry_msgs::msg::Pose & currentPose)
    {
      double distance;
      distance = sqrt(pow(currentPose.position.x - goalPose.pose.position.x, 2) + pow(currentPose.position.y - goalPose.pose.position.y, 2));
      return distance;
    }

    // 计算当前点与障碍物点的距离
    double distance(pcl::PointXYZ x1, pcl::PointXYZ x2)
    {
      double distance;
      distance = sqrt(pow(x1.x - x2.x, 2) + pow(x1.y - x2.y, 2));
      return distance;
    }

    void findOrientation(tf2::Vector3 & cross)
    {
      pcl::PointXYZ searchPoint;
      searchPoint.x = tempPose.pose.position.x;
      searchPoint.y = tempPose.pose.position.y;
      std::vector<int> pointIdxNKNSearch(nearPointsNum);
      std::vector<float> pointNKNSquaredDistance(nearPointsNum);
    
      double crossX, crossY;
      double crossLastX = cross.getX();
      double crossLastY = cross.getY();
      
      //搜索最近的K个点
      if(kdtree.nearestKSearch(searchPoint, nearPointsNum, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
      {
        double currentForceX, currentForceY;
        double force;
        double forceX, forceY;
        double repulsionX = 0, repulsionY = 0;
        double attractForce, attractForceX, attractForceY;
        for(int i = 0; i < nearPointsNum; i++)
        {
          /* 计算受力的合力，对于斥力来说模型是类似于电磁力的斥力，与距离平方成反比 */
          force = double(repulsion / pow(pointNKNSquaredDistance[i], 2));
          forceX = force * (searchPoint.x - laserCloudDwz->points[pointIdxNKNSearch[i]].x) / distance(searchPoint, laserCloudDwz->points[pointIdxNKNSearch[i]]);
          forceY = force * (searchPoint.y - laserCloudDwz->points[pointIdxNKNSearch[i]].y) / distance(searchPoint, laserCloudDwz->points[pointIdxNKNSearch[i]]);
          repulsionX += forceX;
          repulsionY += forceY;
        }
        /* 对于引力，是与距离成正比的，也就是说可以把环境理解为抛物面模型 */
        attractForce = attraction * distanceToGoal(tempPose.pose);
        // attractForce = attraction * 2.0;
        attractForceX = attractForce * (goalPose.pose.position.x - searchPoint.x)/distanceToGoal(tempPose.pose);
        attractForceY = attractForce * (goalPose.pose.position.y - searchPoint.y)/distanceToGoal(tempPose.pose);

        // 计算斥力在引力的投影
        double dotProduct = repulsionX*attractForceX + repulsionY*attractForceY;
        double magnitude = attractForceX*attractForceX + attractForceY*attractForceY;
        double touX=(dotProduct/magnitude) * attractForceX;
        double touY=(dotProduct/magnitude) * attractForceY;

        currentForceX = attractForceX + repulsionX;
        currentForceY = attractForceY + repulsionY;
        // 如果投影力大于引力，且投影与引力方向相反，障碍物增益
        if(sqrt(pow(touX, 2) + pow(touY, 2))>inflationPower*attractForce)
        {
          // currentForceX = repulsionX;
          // currentForceY = repulsionY;
        }
        else if(sqrt(pow(touX, 2) + pow(touY, 2))>attractForce && (touX*attractForceX<0) )
        {
          // 只考虑切向上的力
          repulsionX=repulsionX-touX+attractForceX;
          repulsionY=repulsionY-touY+attractForceY;
          currentForceX = attractForceX + repulsionX;
          currentForceY = attractForceY + repulsionY;
        }

          
        double goal_angle=atan2(attractForceY,attractForceX);
    
        crossX = currentForceX / sqrt(pow(currentForceX, 2) + pow(currentForceY, 2));
        crossY = currentForceY / sqrt(pow(currentForceX, 2) + pow(currentForceY, 2));
        
        tf2::Vector3 temp(crossX, crossY, 0);
        cross = temp;
      }
      else
      {
        RCLCPP_INFO(this->get_logger(), "No neighbor?");
      }
      
    }

    void timer_callback()
    {
      //如果没有新的点云更新或目标
      if(!newLaserCloud || !hasGoal)
      {
        return;
      }

      // 获取初始点（获取base_link在map坐标系下的位置）
      geometry_msgs::msg::TransformStamped transform;
      try
      {
        transform =tf_buffer_->lookupTransform("map", "base_link", tf2::TimePointZero);
      }
      catch (tf2::TransformException &ex)
      {
        RCLCPP_INFO(this->get_logger(), "Received no");
        return;
      }
      tempPose.pose.position.x = transform.transform.translation.x;
      tempPose.pose.position.y = transform.transform.translation.y;

      // 获取目标点的位置
      if (globalPath.poses.size() > 0) 
      {
        for (int i = globalPath.poses.size() - 1; i >= 0; i--) {
            double diff_x = globalPath.poses[i].pose.position.x - transform.transform.translation.x;
            double diff_y = globalPath.poses[i].pose.position.y - transform.transform.translation.y;
            double dis = sqrt(diff_y * diff_y + diff_x * diff_x);
            
            if (dis < planRange) 
            {
              goalPose=globalPath.poses[i];
              break;
            }
        }
      }
      else
      {
        return;
      }
      // 把点云转成kdtree，用于临近点搜索
      kdtree.setInputCloud(laserCloudDwz);
      geometry_msgs::msg::Twist cmd_vel_msg;
      nav_msgs::msg::Path path;
      nav_msgs::msg::Path back_path;

      // 把初始点推送到path
      path.header.frame_id = "map";
      path.poses.push_back(tempPose);

      // 计算初始点到目标点的位置
      // 如果到目标点就发0速度
      double distance = distanceToGoal(tempPose.pose);
      if(distance < goalTolerance)
      {
        publisher_vel->publish(cmd_vel_msg);
        return;
      }

      // 初始化方向   
      tf2::Vector3 cross;
      // 初始化缓存
      geometry_msgs::msg::PoseStamped temp;

      for(int i=0;i<200;i++)
      {
        // if(i==199)
        // {
        //   //如果路径生成失败，不动（表演的时候开这个）
        //   publisher_vel->publish(cmd_vel_msg);
        //   return;
        // }

        //前向传播
        // 获取下一个点的方向
        findOrientation(cross);
        tempPose.pose.position.x = tempPose.pose.position.x + searchStep * cross.getX();
        tempPose.pose.position.y = tempPose.pose.position.y + searchStep * cross.getY();
        // RCLCPP_INFO(this->get_logger(), "%f, %f",cross.getY(),cross.getX());
        path.poses.push_back(tempPose);
        distance = distanceToGoal(tempPose.pose);
        if(distance < goalTolerance)
        {
          break;
        }
          

        //反向传播

        //换位
        temp=tempPose;
        tempPose=goalPose;
        goalPose=temp;

        // 获取下一个点的方向
        findOrientation(cross);
        tempPose.pose.position.x = tempPose.pose.position.x + searchStep * cross.getX();
        tempPose.pose.position.y = tempPose.pose.position.y + searchStep * cross.getY();
        // RCLCPP_INFO(this->get_logger(), "%f, %f",cross.getY(),cross.getX());
        back_path.poses.push_back(tempPose);
        distance = distanceToGoal(tempPose.pose);
        if(distance < goalTolerance)
        {
          break;
        }

        //换位
        temp=tempPose;
        tempPose=goalPose;
        goalPose=temp;
        
      }

      // 推送返回路径
      if (back_path.poses.size() > 0) 
      {
        for (int i = back_path.poses.size() - 1; i >= 0; i--) 
        {
          path.poses.push_back(back_path.poses[i]);
        }
      }
      
      double diff_x=0;   
      double diff_y=0;

      // 获取目标点的位置
      if (path.poses.size() > 0) 
      {
        for (int i = 0; i < path.poses.size(); i++) {
            diff_x = path.poses[i].pose.position.x - path.poses[0].pose.position.x;
            diff_y = path.poses[i].pose.position.y - path.poses[0].pose.position.y;
            double dis = sqrt(diff_y * diff_y + diff_x * diff_x);
            
            if (dis > 0.4 && dis<1.0) 
            {
              break;
            }
        }
      }

      //发布速度
      cmd_vel_msg.linear.x=std::max(abs(diff_x*speed),minSpeed);
      cmd_vel_msg.linear.y=std::max(abs(diff_y*speed),minSpeed);
      if(diff_x<0)
        cmd_vel_msg.linear.x=-cmd_vel_msg.linear.x;
      if(diff_y<0)
        cmd_vel_msg.linear.y=-cmd_vel_msg.linear.y;
     
      publisher_vel->publish(cmd_vel_msg);

      //debug
      if(isDebug)
        publisher_path->publish(path);
    }

        /**
    * @brief 计算每个点0.05范围内的坡度和坡的方向，并添加到新的点云中
    *
    * @param input_cloud 输入的点云 pcl::PointCloud<pcl::PointXYZ>
    * @return pcl::PointCloud<pcl::PointXYZI> 包含坡度和坡方向的新点云
    */
    // void calculateSlopeAndDirection() 
    // {
    //     laserCloudI->clear();
    //     kdtree.setInputCloud(laserCloudDwz);

    //     for (size_t i = 0; i < laserCloudDwz->points.size(); ++i) {
    //         pcl::PointXYZ current_point = laserCloudDwz->points[i];
    //         double sum_x = 0.0;
    //         double sum_y = 0.0;
    //         double sum_z = 0.0;
    //         int num_points = 0;

    //         std::vector<int> pointIdxRadiusSearch;
    //         std::vector<float> pointRadiusSquaredDistance;

    //         // 使用k邻近搜索来查找voxelGridResolution范围内的点
    //         if (kdtree.radiusSearch(current_point,voxelGridResolution, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0) {
    //             for (size_t j = 0; j < pointIdxRadiusSearch.size(); ++j) {
    //                 if (pointIdxRadiusSearch[j] == static_cast<int>(i)) continue;

    //                 double dz = laserCloudDwz->points[pointIdxRadiusSearch[j]].z - current_point.z;
    //                 double dx = laserCloudDwz->points[pointIdxRadiusSearch[j]].x - current_point.x;
    //                 double dy = laserCloudDwz->points[pointIdxRadiusSearch[j]].y - current_point.y;
    //                 sum_z+=dz;
    //                 sum_x+=dx;
    //                 sum_y+=dy;
                    
    //                 num_points++;
    //             }
    //         }

    //         if (num_points > 0) {
    //             double average_x = sum_x / num_points;
    //             double average_y = sum_y / num_points;
    //             double average_z = sum_z / num_points;
    //             if(abs(average_z)>0.05)
    //             {

    //               pcl::PointXYZI new_point;
    //               new_point.x = current_point.x;
    //               new_point.y = current_point.y;
    //               new_point.z = current_point.z;

    //               // 将整数部分作为x方向坡度，小数部分作为y方向坡度添加到新点云中
    //               new_point.intensity = average_z;

    //               laserCloudI->push_back(new_point);

    //             }
    //         }
    //     }
    // }

  private:
    //话题接收
    rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr goal_sub;
    rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub;
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_sub;
    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr publisher_path;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_cloud;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_vel;
    rclcpp::TimerBase::SharedPtr timer_;
    // Subscriber with tf2
    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
    geometry_msgs::msg::TransformStamped transform;
};



int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<ApfPlanner>();
    rclcpp::spin(node);
    rclcpp::shutdown();

    return 0;
}